#include "core/Core.h"


Core::Core(Bus &bus) : bus(bus) {
    this->reset();
}

void Core::fetch(Package &pkg) {
    bool mmio;
    pkg.pc = pc;
    if (pc & 0x1) { take_trap(Trap::InstructionAddressMisaligned, pc); }
    if (!bus.read(pc, 4, (uint8_t *)&pkg.inst_val, mmio)) {
        take_trap(Trap::InstructionAccessFault, pc);
    }
}

void Core::decode(Package &pkg) {
    uint32_t inst = pkg.inst_val;
    pkg.opcode = inst & 0x0000007F;
    pkg.rd = (inst >> 7) & 0x0000001F;
    pkg.rs1 = (inst >> 15) & 0x0000001F;
    pkg.rs2 = (inst >> 20) & 0x0000001F;
    pkg.funct3 = (inst >> 12) & 0x00000007;
    pkg.funct7 = (inst >> 25) & 0x0000007F;
    pkg.shamt = (inst >> 20) & 0x0000003F;
    pkg.csr = (inst >> 20) & 0x00000FFF;
    pkg.zimm = pkg.rs1;

    switch (pkg.opcode) {
        case OPCODE_LUI:
            pkg.inst = Instruction::LUI;
            pkg.imm = imm_U(inst);
            break;
        case OPCODE_AUIPC:
            pkg.inst = Instruction::AUIPC;
            pkg.imm = imm_U(inst);
            break;
        case OPCODE_JAL:
            pkg.inst = Instruction::JAL;
            pkg.imm = imm_J(inst);
            break;
        case OPCODE_JALR:
            pkg.inst = Instruction::JALR;
            pkg.imm = imm_I(inst);
            break;
        case OPCODE_BRANCH:
            pkg.imm = imm_B(inst);
            switch (pkg.funct3) {
                case FUNCT3_BEQ:
                    pkg.inst = Instruction::BEQ;
                    break;
                case FUNCT3_BNE:
                    pkg.inst = Instruction::BNE;
                    break;
                case FUNCT3_BLT:
                    pkg.inst = Instruction::BLT;
                    break;
                case FUNCT3_BGE:
                    pkg.inst = Instruction::BGE;
                    break;
                case FUNCT3_BLTU:
                    pkg.inst = Instruction::BLTU;
                    break;
                case FUNCT3_BGEU:
                    pkg.inst = Instruction::BGEU;
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        case OPCODE_LOAD:
            pkg.imm = imm_I(inst);
            switch (pkg.funct3) {
                case FUNCT3_LB:
                    pkg.inst = Instruction::LB;
                    break;
                case FUNCT3_LH:
                    pkg.inst = Instruction::LH;
                    break;
                case FUNCT3_LW:
                    pkg.inst = Instruction::LW;
                    break;
                case FUNCT3_LD:
                    pkg.inst = Instruction::LD;
                    break;
                case FUNCT3_LBU:
                    pkg.inst = Instruction::LBU;
                    break;
                case FUNCT3_LHU:
                    pkg.inst = Instruction::LHU;
                    break;
                case FUNCT3_LWU:
                    pkg.inst = Instruction::LWU;
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        case OPCODE_STORE:
            pkg.imm = imm_S(inst);
            switch (pkg.funct3) {
                case FUNCT3_SB:
                    pkg.inst = Instruction::SB;
                    break;
                case FUNCT3_SH:
                    pkg.inst = Instruction::SH;
                    break;
                case FUNCT3_SW:
                    pkg.inst = Instruction::SW;
                    break;
                case FUNCT3_SD:
                    pkg.inst = Instruction::SD;
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        case OPCODE_OPIMM:
            pkg.imm = imm_I(inst);
            switch (pkg.funct3) {
                case FUNCT3_ADD_SUB:
                    pkg.inst = Instruction::ADDI;
                    break;
                case FUNCT3_SLL:
                    pkg.inst = Instruction::SLLI;
                    Assert(pkg.funct7 >> 1 == 0, "SLLI");
                    break;
                case FUNCT3_SLT:
                    pkg.inst = Instruction::SLTI;
                    break;
                case FUNCT3_SLTU:
                    pkg.inst = Instruction::SLTIU;
                    break;
                case FUNCT3_XOR:
                    pkg.inst = Instruction::XORI;
                    break;
                case FUNCT3_SRL_SRA:
                    switch (pkg.funct7 >> 1) {
                        case 0b000000:
                            pkg.inst = Instruction::SRLI;
                            break;
                        case 0b010000:
                            pkg.inst = Instruction::SRAI;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                case FUNCT3_OR:
                    pkg.inst = Instruction::ORI;
                    break;
                case FUNCT3_AND:
                    pkg.inst = Instruction::ANDI;
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        case OPCODE_OPIMM32:
            pkg.imm = imm_I(inst);
            switch (pkg.funct3) {
                case FUNCT3_ADD_SUB:
                    pkg.inst = Instruction::ADDIW;
                    break;
                case FUNCT3_SLL:
                    pkg.inst = Instruction::SLLIW;
                    Assert(pkg.funct7 >> 1 == 0, "SLLIW");
                    break;
                case FUNCT3_SRL_SRA:
                    switch (pkg.funct7 >> 1) {
                        case 0b000000:
                            pkg.inst = Instruction::SRLIW;
                            break;
                        case 0b010000:
                            pkg.inst = Instruction::SRAIW;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        case OPCODE_OP:
            switch (pkg.funct7) {
                case FUNCT7_NORMAL: 
                    switch (pkg.funct3) {
                        case FUNCT3_ADD_SUB:
                            pkg.inst = Instruction::ADD;
                            break;
                        case FUNCT3_SLL:
                            pkg.inst = Instruction::SLL;
                            break;
                        case FUNCT3_SLT:
                            pkg.inst = Instruction::SLT;
                            break;
                        case FUNCT3_SLTU:
                            pkg.inst = Instruction::SLTU;
                            break;
                        case FUNCT3_XOR:
                            pkg.inst = Instruction::XOR;
                            break;
                        case FUNCT3_SRL_SRA:
                            pkg.inst = Instruction::SRL;
                            break;
                        case FUNCT3_OR:
                            pkg.inst = Instruction::OR;
                            break;
                        case FUNCT3_AND:
                            pkg.inst = Instruction::AND;
                            break; 
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                case FUNCT7_SUB_SRA:
                    switch (pkg.funct3) {
                        case FUNCT3_ADD_SUB:
                            pkg.inst = Instruction::SUB;
                            break;
                        case FUNCT3_SRL_SRA:
                            pkg.inst = Instruction::SRA;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                case FUNCT7_MUL:
                    switch (pkg.funct3) {
                        case FUNCT3_MUL:
                            pkg.inst = Instruction::MUL;
                            break;
                        case FUNCT3_MULH:
                            pkg.inst = Instruction::MULH;
                            break;
                        case FUNCT3_MULHSU:
                            pkg.inst = Instruction::MULHSU;
                            break;
                        case FUNCT3_MULHU:
                            pkg.inst = Instruction::MULHU;
                            break;
                        case FUNCT3_DIV:
                            pkg.inst = Instruction::DIV;
                            break;
                        case FUNCT3_DIVU:
                            pkg.inst = Instruction::DIVU;
                            break;
                        case FUNCT3_REM:
                            pkg.inst = Instruction::REM;
                            break;
                        case FUNCT3_REMU:
                            pkg.inst = Instruction::REMU;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        case OPCODE_OP32:
            switch (pkg.funct7) {
                case FUNCT7_NORMAL: 
                    switch (pkg.funct3) {
                        case FUNCT3_ADD_SUB:
                            pkg.inst = Instruction::ADDW;
                            break;
                        case FUNCT3_SLL:
                            pkg.inst = Instruction::SLLW;
                            break;
                        case FUNCT3_SRL_SRA:
                            pkg.inst = Instruction::SRLW;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                case FUNCT7_SUB_SRA:
                    switch (pkg.funct3) {
                        case FUNCT3_ADD_SUB:
                            pkg.inst = Instruction::SUBW;
                            break;
                        case FUNCT3_SRL_SRA:
                            pkg.inst = Instruction::SRAW;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                case FUNCT7_MUL:
                    switch (pkg.funct3) {
                        case FUNCT3_MUL:
                            pkg.inst = Instruction::MULW;
                            break;
                        case FUNCT3_DIV:
                            pkg.inst = Instruction::DIVW;
                            break;
                        case FUNCT3_DIVU:
                            pkg.inst = Instruction::DIVUW;
                            break;
                        case FUNCT3_REM:
                            pkg.inst = Instruction::REMW;
                            break;
                        case FUNCT3_REMU:
                            pkg.inst = Instruction::REMUW;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        case OPCODE_SYSTEM:
            switch (pkg.funct3) {
                case FUNCT3_PRIV:
                    switch (pkg.funct7) {
                        // ebreak & ecall
                        case 0b0000000:
                            if (pkg.rs1 == 0 && pkg.rd == 0) {
                                switch (pkg.rs2) {
                                    case 0x000:
                                        pkg.inst = Instruction::ECALL;
                                        break;
                                    case 0x001:
                                        pkg.inst = Instruction::EBREAK;
                                        break;
                                    default: take_trap(Trap::IllegalInstruction, pc);
                                }
                            } else {
                                take_trap(Trap::IllegalInstruction, pc);
                            }
                            break;
                        // mret
                        case 0b0011000:
                            pkg.inst = Instruction::MRET;
                            break;
                        default:
                            take_trap(Trap::IllegalInstruction, pc);
                    }
                    break;
                case FUNCT3_CSRRW:
                    pkg.inst = Instruction::CSRRW;
                    break;
                case FUNCT3_CSRRS:
                    pkg.inst = Instruction::CSRRS;
                    break;
                case FUNCT3_CSRRC:
                    pkg.inst = Instruction::CSRRC;
                    break;
                case FUNCT3_CSRRWI:
                    pkg.inst = Instruction::CSRRWI;
                    break;
                case FUNCT3_CSRRSI:
                    pkg.inst = Instruction::CSRRSI;
                    break;
                case FUNCT3_CSRRCI:
                    pkg.inst = Instruction::CSRRCI;
                    break;
                default:
                    take_trap(Trap::IllegalInstruction, pc);
            }
            break;
        default:
            take_trap(Trap::IllegalInstruction, pc);
    }
}

void Core::execute(Package &pkg) {
    word_t temp = 0;
    bool mmio = false;
    word_t npc = pc + 4;
    mstatus_t* mstatus = reinterpret_cast<mstatus_t*>(&csr[MSTATUS]);
    switch (pkg.inst) {
        case Instruction::LUI:
            gpr[pkg.rd] = pkg.imm;
            break;
        case Instruction::AUIPC:
            gpr[pkg.rd] = pc + pkg.imm;
            break;
        case Instruction::JAL:
            gpr[pkg.rd] = pc + 4;
            npc = pc + pkg.imm;
            break;
        case Instruction::JALR:
            temp = pc + 4;
            npc = (gpr[pkg.rs1] + pkg.imm) & ~1;
            gpr[pkg.rd] = temp;
            break;
        case Instruction::BEQ:
            if (gpr[pkg.rs1] == gpr[pkg.rs2]) { npc = pc + pkg.imm; }
            break;
        case Instruction::BNE:
            if (gpr[pkg.rs1] != gpr[pkg.rs2]) { npc = pc + pkg.imm; }
            break;
        case Instruction::BLT:
            if (static_cast<sword_t>(gpr[pkg.rs1]) < static_cast<sword_t>(gpr[pkg.rs2])) { npc = pc + pkg.imm; }
            break;
        case Instruction::BGE:
            if (static_cast<sword_t>(gpr[pkg.rs1]) >= static_cast<sword_t>(gpr[pkg.rs2])) { npc = pc + pkg.imm; }
            break;
        case Instruction::BLTU:
            if (gpr[pkg.rs1] < gpr[pkg.rs2]) { npc = pc + pkg.imm; }
            break;
        case Instruction::BGEU:
             if (gpr[pkg.rs1] >= gpr[pkg.rs2]) { npc = pc + pkg.imm; }
            break;
        case Instruction::LB:
            gpr[pkg.rd] =
                static_cast<int8_t>(this->load(gpr[pkg.rs1] + pkg.imm, 1, mmio));
            break;
        case Instruction::LH:
            gpr[pkg.rd] =
                static_cast<int16_t>(this->load(gpr[pkg.rs1] + pkg.imm, 2, mmio));
            break;
        case Instruction::LW:
            gpr[pkg.rd] =
                static_cast<int32_t>(this->load(gpr[pkg.rs1] + pkg.imm, 4, mmio));
            break;
        case Instruction::LD:
            gpr[pkg.rd] =
                static_cast<int64_t>(this->load(gpr[pkg.rs1] + pkg.imm, 8, mmio));
            break;
        case Instruction::LBU:
            gpr[pkg.rd] =
                static_cast<uint8_t>(this->load(gpr[pkg.rs1] + pkg.imm, 1, mmio));
            break;
        case Instruction::LHU:
            gpr[pkg.rd] =
                static_cast<uint16_t>(this->load(gpr[pkg.rs1] + pkg.imm, 2, mmio));
            break;
        case Instruction::LWU:
            gpr[pkg.rd] =
                static_cast<uint32_t>(this->load(gpr[pkg.rs1] + pkg.imm, 4, mmio));
            break;
        case Instruction::SB:
            this->store(gpr[pkg.rs1] + pkg.imm, 1, gpr[pkg.rs2], mmio);
            break;
        case Instruction::SH:
            this->store(gpr[pkg.rs1] + pkg.imm, 2, gpr[pkg.rs2], mmio);
            break;
        case Instruction::SW:
            this->store(gpr[pkg.rs1] + pkg.imm, 4, gpr[pkg.rs2], mmio);
            break;
        case Instruction::SD:
            this->store(gpr[pkg.rs1] + pkg.imm, 8, gpr[pkg.rs2], mmio);
            break;
        case Instruction::ADDI:
            gpr[pkg.rd] = gpr[pkg.rs1] + pkg.imm;
            break;
        case Instruction::SLLI:
            gpr[pkg.rd] = gpr[pkg.rs1] << pkg.shamt;
            break;
        case Instruction::SLTI:
            gpr[pkg.rd] =
                static_cast<sword_t>(gpr[pkg.rs1]) < static_cast<sword_t>(pkg.imm);
            break;
        case Instruction::SLTIU:
            gpr[pkg.rd] = gpr[pkg.rs1] < pkg.imm;
            break;
        case Instruction::XORI:
            gpr[pkg.rd] = gpr[pkg.rs1] ^ pkg.imm;
            break;
        case Instruction::SRLI:
            gpr[pkg.rd] = gpr[pkg.rs1] >> pkg.shamt;
            break;
        case Instruction::SRAI:
            gpr[pkg.rd] = static_cast<sword_t>(gpr[pkg.rs1]) >> pkg.shamt;
            break;
        case Instruction::ORI:
            gpr[pkg.rd] = gpr[pkg.rs1] | pkg.imm;
            break;
        case Instruction::ANDI:
            gpr[pkg.rd] = gpr[pkg.rs1] & pkg.imm;
            break;
        case Instruction::ADD:
            gpr[pkg.rd] = gpr[pkg.rs1] + gpr[pkg.rs2];
            break;
        case Instruction::SUB:
            gpr[pkg.rd] = gpr[pkg.rs1] - gpr[pkg.rs2];
            break;
        case Instruction::SLL:
            gpr[pkg.rd] = gpr[pkg.rs1] << gpr[pkg.rs2];
            break;
        case Instruction::SLT:
            gpr[pkg.rd] =
                static_cast<sword_t>(gpr[pkg.rs1]) < static_cast<sword_t>(gpr[pkg.rs2]);
            break;
        case Instruction::SLTU:
            gpr[pkg.rd] = gpr[pkg.rs1] < gpr[pkg.rs2];
            break;
        case Instruction::XOR:
            gpr[pkg.rd] = gpr[pkg.rs1] ^ gpr[pkg.rs2];
            break;
        case Instruction::SRL:
            gpr[pkg.rd] = gpr[pkg.rs1] >> gpr[pkg.rs2];
            break;
        case Instruction::SRA:
            gpr[pkg.rd] = static_cast<sword_t>(gpr[pkg.rs1]) >> gpr[pkg.rs2];
            break;
        case Instruction::OR:
            gpr[pkg.rd] = gpr[pkg.rs1] | gpr[pkg.rs2];
            break;
        case Instruction::AND:
            gpr[pkg.rd] = gpr[pkg.rs1] & gpr[pkg.rs2];
            break;
        case Instruction::MUL:
            gpr[pkg.rd] = static_cast<sword_t>(gpr[pkg.rs1]) * static_cast<sword_t>(gpr[pkg.rs2]);
            break;
        case Instruction::MULH: /// ????
            gpr[pkg.rd] = static_cast<sdword_t>(static_cast<sdword_t>(gpr[pkg.rs1]) * static_cast<sdword_t>(gpr[pkg.rs2])) >> XLEN;
            break;
        case Instruction::MULHSU: /// ????
            gpr[pkg.rd] = static_cast<sdword_t>(static_cast<sdword_t>(gpr[pkg.rs1]) * static_cast<dword_t>(gpr[pkg.rs2])) >> XLEN;
            break;
        case Instruction::MULHU: /// ????
            gpr[pkg.rd] = static_cast<dword_t>(static_cast<dword_t>(gpr[pkg.rs1]) * static_cast<dword_t>(gpr[pkg.rs2])) >> XLEN;
            break;
        case Instruction::DIV:
            gpr[pkg.rd] = static_cast<sword_t>(gpr[pkg.rs1]) / static_cast<sword_t>(gpr[pkg.rs2]);
            break;
        case Instruction::DIVU:
            gpr[pkg.rd] = gpr[pkg.rs1] / gpr[pkg.rs2];
            break;
        case Instruction::REM:
            gpr[pkg.rd] = static_cast<sword_t>(gpr[pkg.rs1]) % static_cast<sword_t>(gpr[pkg.rs2]);
            break;
        case Instruction::REMU:
            gpr[pkg.rd] = gpr[pkg.rs1] % gpr[pkg.rs2];
            break;
        case Instruction::ECALL:
            npc = take_trap(Trap::EnvironmentCallFromMMode, pc);
            break;
        case Instruction::EBREAK:
            take_trap(Trap::Breakpoint, pc);
            break;
        case Instruction::MRET:
            mstatus = reinterpret_cast<mstatus_t*>(&csr[MSTATUS]);
            npc = csr[MEPC];
            mstatus->MIE = mstatus->MPIE;
            mstatus->MPIE = 1;
            mstatus->MPP = 0b11;
            break;
        case Instruction::CSRRW:
            temp = csr[pkg.csr];
            csr[pkg.csr] = gpr[pkg.rs1];
            gpr[pkg.rd] = temp;
            break;
        case Instruction::CSRRS:
            temp = csr[pkg.csr];
            csr[pkg.csr] = temp | gpr[pkg.rs1];
            gpr[pkg.rd] = temp;
            break;
        case Instruction::CSRRC:
            temp = csr[pkg.csr];
            csr[pkg.csr] = temp & ~gpr[pkg.rs1];
            gpr[pkg.rd] = temp;
            break;
        case Instruction::CSRRWI:
            gpr[pkg.rd] = csr[pkg.csr];
            csr[pkg.csr] = pkg.zimm;
            break;
        case Instruction::CSRRSI:
            temp = csr[pkg.csr];
            csr[pkg.csr] = temp | pkg.zimm;
            gpr[pkg.rd] = temp;
            break;
        case Instruction::CSRRCI:
            temp = csr[pkg.csr];
            csr[pkg.csr] = temp & ~pkg.zimm;
            gpr[pkg.rd] = temp;
            break;
        case Instruction::ADDIW:
            gpr[pkg.rd] = static_cast<int32_t>(gpr[pkg.rs1] + pkg.imm);
            break;
        case Instruction::SLLIW:
            gpr[pkg.rd] = static_cast<int32_t>(gpr[pkg.rs1] << pkg.shamt);
            break;
        case Instruction::SRLIW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<uint32_t>(gpr[pkg.rs1]) >> pkg.shamt);
            break;
        case Instruction::SRAIW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<int32_t>(gpr[pkg.rs1]) >> pkg.shamt);
            break;
        case Instruction::ADDW:
            gpr[pkg.rd] = static_cast<int32_t>(gpr[pkg.rs1] + gpr[pkg.rs2]);
            break;
        case Instruction::SUBW:
            gpr[pkg.rd] = static_cast<int32_t>(gpr[pkg.rs1] - gpr[pkg.rs2]);
            break;
        case Instruction::SLLW:
            gpr[pkg.rd] = static_cast<int32_t>(gpr[pkg.rs1] << (gpr[pkg.rs2] & 0x1F));
            break;
        case Instruction::SRLW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<uint32_t>(gpr[pkg.rs1]) >> (gpr[pkg.rs2] & 0x1F));
            break;
        case Instruction::SRAW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<int32_t>(gpr[pkg.rs1]) >> (gpr[pkg.rs2] & 0x1F));
            break;
        case Instruction::MULW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<int32_t>(gpr[pkg.rs1]) * static_cast<int32_t>(gpr[pkg.rs2]));
            break;
        case Instruction::DIVW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<int32_t>(gpr[pkg.rs1]) / static_cast<int32_t>(gpr[pkg.rs2]));
            break;
        case Instruction::DIVUW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<uint32_t>(gpr[pkg.rs1]) / static_cast<uint32_t>(gpr[pkg.rs2]));
            break;
        case Instruction::REMW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<int32_t>(gpr[pkg.rs1]) % static_cast<int32_t>(gpr[pkg.rs2]));
            break;
        case Instruction::REMUW:
            gpr[pkg.rd] = static_cast<int32_t>(static_cast<uint32_t>(gpr[pkg.rs1]) % static_cast<uint32_t>(gpr[pkg.rs2]));
            break;
        default:
            take_trap(Trap::IllegalInstruction, pc);
    }
    pc = npc;
    gpr[0] = 0; // reset $zero to 0
    IFDEF(CONFIG_DIFFTEST, if (mmio) difftest_skip_ref());
}

word_t Core::load(word_t addr, size_t size, bool& mmio) {
    word_t buf;
    if (!bus.read(addr, size, (uint8_t *)&buf, mmio)) {
        take_trap(Trap::LoadAccessFault, pc);
    }
    return buf;
}

void Core::store(word_t addr, size_t size, word_t data, bool& mmio) {
    if (!bus.write(addr, size, (uint8_t *)&data, mmio)) {
        take_trap(Trap::StoreAMOAccessFault, pc);
    }
}

word_t Core::take_trap(Trap trap_cause, word_t trap_pc) {
    static mstatus_t* mstatus = reinterpret_cast<mstatus_t*>(&csr[MSTATUS]);

    csr[MEPC] = trap_pc;
    csr[MCAUSE] = static_cast<word_t>(trap_cause);
    mstatus->MPIE = mstatus->MIE;
    mstatus->MIE = 0;

    if (fatal_trap(trap_cause)) {
        throw RVException { trap_cause, trap_pc, gpr[10] };
    }
    
    return csr[MTVEC];
}

void Core::step(bool msip, bool mtip) {
    static Package pkg;
    IFDEF(CONFIG_CLINT, this->update_mip(msip, mtip));
    this->fetch(pkg);
    this->decode(pkg);
    this->execute(pkg);
    IFDEF(CONFIG_ITRACE, this->itrace->add(pkg.pc, pkg.inst_val));
    IFDEF(CONFIG_DIFFTEST, difftest_step(pkg.pc));
    this->check_interrupt();
}

inline void Core::update_mip(bool msip, bool mtip) {
    static mip_t* mip = reinterpret_cast<mip_t*>(&csr[MIP]);
    mip->MTIP = mtip;
    mip->MSIP = msip;
}

inline void Core::check_interrupt() {
    static mstatus_t* mstatus = reinterpret_cast<mstatus_t*>(&csr[MSTATUS]);
    static mie_t* mie = reinterpret_cast<mie_t*>(&csr[MIE]);
    static mip_t* mip = reinterpret_cast<mip_t*>(&csr[MIP]);
    if (!mstatus->MIE) return;
    if (mie->MTIE && mip->MTIP) {
        this->pc = take_trap(Trap::MachineTimerInterrupt, this->pc);
        IFDEF(CONFIG_DIFFTEST, difftest_raise_intr((word_t)Trap::MachineTimerInterrupt));
        return;
    }
    if (mie->MSIE && mip->MSIP) {
        this->pc = take_trap(Trap::MachineSoftwareInterrupt, this->pc);
        IFDEF(CONFIG_DIFFTEST, difftest_raise_intr((word_t)Trap::MachineSoftwareInterrupt));
        return;
    }
}

void Core::reset() {
    this->pc = RESET_VECTOR;
    this->gpr.fill(0);
    this->csr.fill(0);
    this->csr[MSTATUS] = 0x1800;
}

void Core::open_trace(const char* log_path) {
    if (log_path) { std::filesystem::create_directory(log_path); }
    const char* path = (log_path == nullptr) ? "" : log_path;
    IFDEF(CONFIG_ITRACE, this->itrace = new ITrace((std::filesystem::path {log_path} / "itrace.log").c_str()));
}
